#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h> 

#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/statistical_outlier_removal.h> 

#include <pcl/filters/extract_indices.h>
#include <iostream>
#include <vector>
#include <ctime>

using namespace std;
using namespace pcl;
void
visualize (PointCloud<PointXYZ>::Ptr source, PointCloud<PointXYZ>::Ptr target);

void
visualize (PointCloud<PointXYZ>::Ptr source, PointCloud<PointXYZ>::Ptr target,PointCloud<PointXYZ>::Ptr point);
// 回调函数处理接收到的点云数据
void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
    // 将ROS的PointCloud2消息转换为PCL的PointCloud
    pcl::PointCloud<pcl::PointXYZ> cloud0;
    pcl::fromROSMsg(*cloud_msg, cloud0);

    // 输出点云中点的数量
    ROS_INFO("Received point cloud with %ld points", cloud0.points.size());
    PointCloud<PointXYZ>::Ptr cloud (new pcl::PointCloud<PointXYZ>);
cloud=cloud0.makeShared();

    PointCloud<PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<PointXYZ>);
PointCloud<PointXYZ>::Ptr cleared (new pcl::PointCloud<PointXYZ>);

PointCloud<PointXYZ>::Ptr nearpoint (new pcl::PointCloud<PointXYZ>);
PointCloud<PointXYZ>::Ptr carepoint (new pcl::PointCloud<PointXYZ>);

PointIndices::Ptr indices(new pcl::PointIndices);


//io::loadPCDFile ("/home/llx/pcds/1.pcd", *cloud);


StatisticalOutlierRemoval<PointXYZ> sor; 
sor.setInputCloud (cloud); 
 
  sor.setMeanK (50);
  sor.setStddevMulThresh (1.5);
sor.filter (*filtered_cloud);

 VoxelGrid<PointXYZ> vg;
  vg.setInputCloud (filtered_cloud);
  vg.setLeafSize (0.1, 0.1, 0.1);
  vg.filter (*filtered_cloud);

pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(filtered_cloud);



pcl::PointXYZ searchPoint;
  searchPoint.x = 0.0;
  searchPoint.y = 0.0;
  searchPoint.z = 0.0;// 假设雷达位于原点

std::vector<int> pointIdxRadiusSearch;           //存储近邻索引
  std::vector<float> pointRadiusSquaredDistance;   //存储近邻对应距离的平方
 float radius=0.5;

ExtractIndices<pcl::PointXYZ> extract;

//int K = 10; // 我们只查找一个最近的点
// std::vector<int> pointIdxNKNSearch(K);
// std::vector<float> pointNKNSquaredDistance(K);
if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )  //执行半径R内近邻搜索方法
  {
    for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
    {
      // std::cout << "    "  <<   cloud->points[ pointIdxRadiusSearch[i] ].x 
      //           << " " << cloud->points[ pointIdxRadiusSearch[i] ].y 
      //           << " " << cloud->points[ pointIdxRadiusSearch[i] ].z 
      //           << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
                indices->indices.push_back(pointIdxRadiusSearch[i]);
    }
      
  }
    extract.setInputCloud(filtered_cloud);
    extract.setIndices(indices);
    extract.setNegative(true); // 保留指定索引的点，删除其他点
    extract.filter(*filtered_cloud); // 应用提取方法，并将结果存储在filtered_cloud中

for (const auto& point : *filtered_cloud) {
        // 计算点的方位角，假设雷达朝向正y轴
        float angle = atan2(point.x, point.z) * 180.0 / M_PI; // 转换为度

        // 保留-30到+30度之间的点
        if (angle >= -30.0 && angle <= 30.0) {
            carepoint->points.push_back(point);
        }
    }

    carepoint->width = filtered_cloud->points.size();
    carepoint->height = 1;
    carepoint->is_dense = true;
cleared=filtered_cloud;
filtered_cloud=carepoint;

pcl::KdTreeFLANN<pcl::PointXYZ> kdtree2;
kdtree2.setInputCloud(filtered_cloud);
int K = 60; // 最近的6个点
std::vector<int> pointIdxNKNSearch(K);
std::vector<float> pointNKNSquaredDistance(K);
// if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
// {
//     std::cout << "最近点的坐标：" << cloud->points[pointIdxNKNSearch[4]].x << " "
//               << cloud->points[pointIdxNKNSearch[4]].y << " "
//               << cloud->points[pointIdxNKNSearch[4]].z << std::endl;
//     std::cout << "距离：" << sqrt(pointNKNSquaredDistance[4]) << std::endl;

//     if (cloud->points[pointIdxNKNSearch[0]].x == 0 && cloud->points[pointIdxNKNSearch[0]].y == 0 && cloud->points[pointIdxNKNSearch[0]].z == 0)
// 		{
// 			indices->indices.push_back(pointIdxNKNSearch[0]); // 将点的索引添加到indices中
// 		}
// }

if ( kdtree2.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )  //执行K近邻搜索
  {
     //打印所有近邻坐标
    for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
    {
      std::cout << "    "  <<   filtered_cloud->points[ pointIdxNKNSearch[i] ].x 
                << " " << filtered_cloud->points[ pointIdxNKNSearch[i] ].y 
                << " " << filtered_cloud->points[ pointIdxNKNSearch[i] ].z 
                << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
    if (!(filtered_cloud->points[pointIdxNKNSearch[i]].x == 0 && filtered_cloud->points[pointIdxNKNSearch[i]].y == 0 && filtered_cloud->points[pointIdxNKNSearch[i]].z == 0))
    {
        nearpoint->push_back(filtered_cloud->points[pointIdxNKNSearch[i]]);

    }
    
    }
  }

    visualize (cloud,cleared, nearpoint);//,

}
void
visualize (PointCloud<PointXYZ>::Ptr source, PointCloud<PointXYZ>::Ptr target,PointCloud<PointXYZ>::Ptr point)
{
  visualization::PCLVisualizer viewer ("Point Cloud Viewer");

  // 创建两个显示窗口
  int v1, v2;
  viewer.createViewPort (0, 0.0, 0.5, 1.0, v1);
  viewer.createViewPort (0.5, 0.0, 1.0, 1.0, v2);
    
  // 设置背景颜色
  viewer.setBackgroundColor (255, 255, 255, v1);
  viewer.setBackgroundColor (255, 255, 255, v2);

  // 给点云添加颜色
  visualization::PointCloudColorHandlerCustom<PointXYZ> source_color (source, 0, 0, 255);  // blue

  visualization::PointCloudColorHandlerCustom<PointXYZ> target_color (target, 0, 0, 255);  // red
  visualization::PointCloudColorHandlerCustom<PointXYZ> point_color (target,  255,0, 0);  // green

  // 添加点云到显示窗口
  viewer.addPointCloud (source, source_color, "source cloud", v1);
  viewer.addPointCloud (target, target_color, "target cloud", v2);
  viewer.addPointCloud (point, point_color, "near cloud", v2);


//   while (!viewer.wasStopped ())
//   {
//     viewer.spinOnce (100);
//     boost::this_thread::sleep (boost::posix_time::microseconds(100000));
//   }
 while (!viewer.wasStopped()) {
        // 每次循环调用内部的重绘函数
        viewer.spinOnce();
    }
}

void
visualize (PointCloud<PointXYZ>::Ptr source, PointCloud<PointXYZ>::Ptr target)
{
  visualization::PCLVisualizer viewer ("Point Cloud Viewer");

  // 创建两个显示窗口
  int v1, v2;
  viewer.createViewPort (0, 0.0, 0.5, 1.0, v1);
  viewer.createViewPort (0.5, 0.0, 1.0, 1.0, v2);
    
  // 设置背景颜色
  viewer.setBackgroundColor (255, 255, 255, v1);
  viewer.setBackgroundColor (255, 255, 255, v2);

  // 给点云添加颜色
  visualization::PointCloudColorHandlerCustom<PointXYZ> source_color (source, 0, 0, 255);  // blue

  visualization::PointCloudColorHandlerCustom<PointXYZ> target_color (target, 255, 0, 0);  // red


  // 添加点云到显示窗口
  viewer.addPointCloud (source, source_color, "source cloud", v1);
  viewer.addPointCloud (target, target_color, "target cloud", v2);


//   while (!viewer.wasStopped ())
//   {
//     viewer.spinOnce (100);
//     boost::this_thread::sleep (boost::posix_time::microseconds(100000));
//   }
 while (!viewer.wasStopped()) {
        // 每次循环调用内部的重绘函数
        viewer.spinOnce();
    }
}

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "point_cloud_subscriber");
    ros::NodeHandle nh;

    // 创建一个Subscriber，订阅名为"/velodyne_points"的话题
    ros::Subscriber sub = nh.subscribe("/iris_0/velodyne_points", 1, pointCloudCallback);

    // 让ROS接管控制权
    ros::spin();

    return 0;
}
